package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;

/**
 *
 * @author TJ
 */
public class RobotDrive {
    
    private DoubleVictor leftVictors, rightVictors;
    private Encoder leftEncoder, rightEncoder;
    private Solenoid leftSolenoidOn, leftSolenoidOff, rightSolenoidOn, rightSolenoidOff;
    private PIDController leftPID, rightPID;
    private Compressor compressor;
    private DriveType drive;
    private boolean pid;
    private int shiftSetting;
    private boolean shiftState;
    private BeltSystem beltSystem;
    private Relay rampArmRelay;

    public RobotDrive(DriveType drive) {
        this.drive = drive;

        leftVictors = new DoubleVictor(Definitions.DRIVE_DIGITAL_MODULE, Definitions.LEFT_VICTOR_1,
                Definitions.DRIVE_DIGITAL_MODULE, Definitions.LEFT_VICTOR_2);
        rightVictors = new DoubleVictor(Definitions.DRIVE_DIGITAL_MODULE, Definitions.RIGHT_VICTOR_1,
                Definitions.DRIVE_DIGITAL_MODULE, Definitions.RIGHT_VICTOR_2);
        leftEncoder = new Encoder(Definitions.DRIVE_DIGITAL_MODULE, Definitions.LEFT_ENCODER_A, 
                Definitions.DRIVE_DIGITAL_MODULE, Definitions.LEFT_ENCODER_B);
        rightEncoder = new Encoder(Definitions.DRIVE_DIGITAL_MODULE, Definitions.RIGHT_ENCODER_A,
                Definitions.DRIVE_DIGITAL_MODULE, Definitions.RIGHT_ENCODER_B);

        beltSystem = BeltSystem.INSTANCE;
        rampArmRelay = new Relay(Definitions.PAYLOAD_DIGITAL_MODULE, Definitions.RAMP_ARM);
    }

    public void setDrive(DriveType drive) {
        this.drive = drive;
    }

    public void update() {        
        shiftSetting = drive.getShiftSetting();

//	if(newInfo.isPID != info.isPID)
//	{
//		if(newInfo.isPID)
//		{
//			this.initPID();
//		}
//		else
//		{
//			killPID();
//		}
//	}

	if(drive.isPickup()) {
            beltSystem.pickup();
        }
        
	if(drive.getRampArm() > 0) {
            rampArmRelay.set(Relay.Value.kForward);
        } else if(drive.getRampArm() < 0) {
            rampArmRelay.set(Relay.Value.kReverse);
        } else {
            rampArmRelay.set(Relay.Value.kOff);
        }
        
        driveUpdate();
        shiftUpdate();
    }

    private void initPID() {

    }

    private void killPID() {

    }

    private void shiftUpdate() {

    }

    private void driveUpdate() {
        leftVictors.set(-drive.getLeftSpeed() * .4);
        rightVictors.set(drive.getRightSpeed() * .4);
    }

    private void shift(boolean _high) {

    }

    private boolean getAutoShiftHigh() 
    {
        return false;
    }
}
